#include <stdio.h> 
#include <avr/io.h> 

#define SETPIN(port, bit, val) {if(val) (port) |= 1 << (bit); else (port) &= ~(1 << (bit));}
int8_t motor_speed[4];

void motor_init(){
	// motor out
	DDRB = 0xff;
	DDRD = 0xff;

	// clocks on
	TCCR0A = (_BV(WGM00)) | (_BV(WGM01)) | (_BV(COM0A1)) | (_BV(COM0B1)); //Set the Timer0 to PWM Fast Mode
	TCCR0B = (_BV(CS01));
	TCCR2A = (_BV(WGM20)) | (_BV(WGM21)) | (_BV(COM2A1)) | (_BV(COM2B1)); //Set the Timer2 to PWM Fast Mode
	TCCR2B = (_BV(CS21));

	// initial duty
	OCR0A = 0;
	OCR0B = 0;
	OCR2A = 0;
	OCR2B = 0;
}
              

int motor_get_speed(int motor){
	return motor_speed[motor];
}

// takes a number between -100 to 100
void motor_set_speed(int motor, int duty){
	// and if duty was negative, reverse =1
	// abs(duty)
	int reverse = 0;
	if(duty < 0){
		reverse = 1;
		duty = -duty;
	}

	// cap the duty
	if(duty>100)
		duty = 100;

	// save the val for get_speed
	motor_speed[motor] = reverse ? -duty : duty;

	// set motor speed and direction
	if(duty <= 2){
		duty = 0;
	}else{
		duty += 155;
	}
	if(duty > 255){
		duty = 255;
	}
	if(motor == 0){
		OCR0A = duty;
		SETPIN(PORTB, 3, reverse);
	}else if(motor == 1){
		OCR0B = duty;
		SETPIN(PORTB, 1, reverse);
	}else if(motor == 2){
		OCR2A = duty;
		SETPIN(PORTB, 2, reverse);
	}else{
		OCR2B = duty;
		SETPIN(PORTB, 0, reverse);
	}
}

